import copy
import copy
import os
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
import pandas as pd
import numpy as np


class Recorder(Node):
    """
    节点订阅控制量和系统状态,将其保存为csv文件
    """

    class State:
        def __init__(
            self,
            gyro: float,
            angle: float,
            gyroz: float,
            speed: float,
            tl: float,
            tr: float,
        ):
            self.gyro = gyro
            self.angle = angle
            self.gyroz = gyroz
            self.speed = speed
            self.tl = tl
            self.tr = tr

    def __init__(self, rate: int):
        super().__init__("eva_node")
        self.state = self.State(0, 0, 0, 0, 0, 0)
        self.states1 = []
        self.rate = rate
        self.declare_parameter("path", "database/record.csv")
        self.path = self.get_parameter("path").value

        self.leftTorqueSub = self.create_subscription(
            Float64, "left_torque", self.leftTorqueCallback, 10
        )
        self.rightTorqueSub = self.create_subscription(
            Float64, "right_torque", self.rightTorqueCallback, 10
        )
        self.speedSub = self.create_subscription(
            Float64, "speed", self.speedCallback, 10
        )
        self.gyroPitchSub = self.create_subscription(
            Float64, "gyro", self.gyroPitchCallback, 10
        )
        self.pitchSub = self.create_subscription(
            Float64, "angle", self.pitchCallback, 10
        )
        self.gyroYawSub = self.create_subscription(
            Float64, "gyroz", self.gyroYawCallback, 10
        )
        self.timer = self.create_timer(1.0 / self.rate, self.update)

    # tl,tr -> x,theta,phi
    def update(self):
        self.states1.append(copy.copy(self.state))

    def save(self):
        if len(self.states1) == 0:
            self.get_logger().info("no data to save")
            return
        df = pd.DataFrame([vars(state) for state in self.states1])
        df.columns = ["gyro", "angle", "gyroz", "speed", "tl", "tr"]
        df.to_csv(self.path)
        self.get_logger().info("save finished:" + os.path.abspath(self.path))

    def leftTorqueCallback(self, msg: Float64):
        self.state.tl = msg.data

    def rightTorqueCallback(self, msg: Float64):
        self.state.tr = msg.data

    def speedCallback(self, msg: Float64):
        self.state.speed = msg.data

    def gyroPitchCallback(self, msg: Float64):
        self.state.gyro = msg.data

    def pitchCallback(self, msg: Float64):
        self.state.angle = msg.data

    def gyroYawCallback(self, msg: Float64):
        self.state.gyroz = msg.data


def main(args=None):
    rclpy.init(args=args)
    eva_node = Recorder(50)
    try:
        rclpy.spin(eva_node)
    except KeyboardInterrupt:
        pass
    finally:
        eva_node.save()


if __name__ == "__main__":
    main()
